#!/usr/bin/env python
import importlib

import rospy
from sensor_msgs.msg import Joy
import time
from geometry_msgs.msg import Twist, Vector3,Point
from math import copysign, sqrt, pow
from std_msgs.msg import String as StringMsg
from rocr6_msgs.msg import Goal,Gripper,xJoint,Feedback
from zeus_s2_msgs.msg import joy_control
import tf

class JoyTeleop:
    def __init__(self):
        self.active = 0
        self.flag = 0
        self.x_speed_scale = rospy.get_param('~x_speed_scale')
        # self.y_speed_scale = rospy.get_param('~y_speed_scale')
        self.w_speed_scale = rospy.get_param('~w_speed_scale')
        self.velocity = Twist()
        self.rocr6 = Goal()
        self.jiazhua = Gripper()
        self.joyps3 = joy_control()
        self.rate = rospy.Rate(20)

        # Set the distance to travel
        self.test_distance = 1.0 # meters
        self.speed = 0.15 # meters per second
        self.tolerance = 0.01 # meters
        self.odom_linear_scale_correction = 1.0
        self.start_test = False
        self.base_frame = rospy.get_param('~base_frame', '/base_footprint')
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')
        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()
        # Give tf some time to fill its buffer
        rospy.sleep(2)
         # Make sure we see the odom and base frames
        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))  
        rospy.loginfo("demo_show_ control the test.")
        
        self.position = Point()
        
        # Get the starting position from the tf transform between the odom and base frames
        self.position = self.get_position()
        
        x_start = self.position.x
        y_start = self.position.y
            
        move_cmd = Twist()
        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)
        
        # self.cmdVelPublisher = rospy.Publisher('/cmd_moment', joy_control, queue_size = 3)
        self.cmdVelPublisher = rospy.Publisher('/cmd_vel', Twist, queue_size = 3)
        self.joySubscriber = rospy.Subscriber('joy', Joy, self.buttonCallback)
        self.rocr6Publisher = rospy.Publisher('/rocr6_msgs/goal', Goal, queue_size = 3)
        self.jiazhuaPublisher = rospy.Publisher('/rocr6_msgs/gripper', Gripper, queue_size = 3)
        self.loop()
    def buttonCallback(self, joy_data):
        # print "X : %f  ,Y : %f  ,Z: %f  ,joy_data.axes[0]: %f ,joy_data.axes[1]: %f  ,joy_data.axes[2]: %f  ,\
        # joy_data.axes[3]: %f ,joy_data.axes[4]: %f ,joy_data.axes[5]: %f,joy_data.buttons[4]:%f,joy_data.buttons[5]:%f" %(self.velocity.linear.x , self.velocity.linear.y,self.velocity.angular.z,joy_data.axes[0],\
        # joy_data.axes[1],joy_data.axes[2], joy_data.axes[3] , joy_data.axes[4], joy_data.axes[5],joy_data.buttons[4] ,joy_data.buttons[5])

        if(joy_data.buttons[6] == 1):
            self.velocity.linear.x = self.x_speed_scale * joy_data.axes[3]
            self.velocity.angular.z = self.w_speed_scale * joy_data.axes[0]
            self.active = 1		
        else:
            self.velocity.linear = Vector3(0.,0.,0.)
            self.velocity.angular = Vector3(0.,0.,0.)
            self.active = 1
            # self.cmdVelPublisher.publish(self.velocity)					

        if(joy_data.axes[6]==1.0):
            self.rocr6.shoulder_pan_joint.position = 0.0
            self.rocr6.shoulder_pan_joint.velocity = 0.0
            self.rocr6.shoulder_pan_joint.effort = 0.0

            self.rocr6.shoulder_lift_joint.position = 1.5235
            self.rocr6.shoulder_lift_joint.velocity = 0.0
            self.rocr6.shoulder_lift_joint.effort = 0.0

            self.rocr6.elbow_joint.position = -2.5235
            self.rocr6.elbow_joint.velocity = 0.0
            self.rocr6.elbow_joint.effort = 0.0

            self.rocr6.wrist_1_joint.position = 0.0
            self.rocr6.wrist_1_joint.velocity = 0.0
            self.rocr6.wrist_1_joint.effort = 0.0

            self.rocr6.wrist_2_joint.position = 0.0
            self.rocr6.wrist_2_joint.velocity = 0.0
            self.rocr6.wrist_2_joint.effort = 0.0

            self.rocr6.wrist_3_joint.position = 1.0
            self.rocr6.wrist_3_joint.velocity = 0.0
            self.rocr6.wrist_3_joint.effort = 0.0
            self.rocr6Publisher.publish(self.rocr6)
        if(joy_data.axes[6]== -1.0):
            self.rocr6.shoulder_pan_joint.position = 0.0
            self.rocr6.shoulder_pan_joint.velocity = 0.0
            self.rocr6.shoulder_pan_joint.effort = 0.0

            self.rocr6.shoulder_lift_joint.position = 0.0
            self.rocr6.shoulder_lift_joint.velocity = 0.0
            self.rocr6.shoulder_lift_joint.effort = 0.0

            self.rocr6.elbow_joint.position = 0.0
            self.rocr6.elbow_joint.velocity = 0.0 
            self.rocr6.elbow_joint.effort = 0.0

            self.rocr6.wrist_1_joint.position = 0.0
            self.rocr6.wrist_1_joint.velocity = 0.0
            self.rocr6.wrist_1_joint.effort = 0.0

            self.rocr6.wrist_2_joint.position = 0.0
            self.rocr6.wrist_2_joint.velocity = 0.0
            self.rocr6.wrist_2_joint.effort = 0.0

            self.rocr6.wrist_3_joint.position = 0.0
            self.rocr6.wrist_3_joint.velocity = 0.0
            self.rocr6.wrist_3_joint.effort = 0.0
            self.rocr6Publisher.publish(self.rocr6)
        if(joy_data.axes[7]==1.0):
            self.jiazhua.state = 1
            self.jiazhuaPublisher.publish(self.jiazhua)
        if(joy_data.axes[7]== -1.0):
            self.jiazhua.state = 0
            self.jiazhuaPublisher.publish(self.jiazhua)							

        if(joy_data.buttons[4]== 1):
            self.start_test = True
            self.test_distance = 1.0 # meters
            self.speed = 0.15 # meters per second
            self.tolerance = 0.01 # meters
             # Stop the robot by default
            self.velocity = Twist()
            self.active = 1

        if(joy_data.buttons[0]== 1):
            self.start_test = True
            self.test_distance = 1.0 # meters
            self.speed = -0.15 # meters per second
            self.tolerance = 0.01 # meters
             # Stop the robot by default
            self.velocity = Twist()
            self.active = 1
    			
                            
    def loop(self):
        while not rospy.is_shutdown():
            if self.start_test:
                # Get the current position from the tf transform between the odom and base frames
                self.position = self.get_position()
                
                # Compute the Euclidean distance from the target point
                distance = sqrt(pow((self.position.x - x_start), 2) +
                                pow((self.position.y - y_start), 2))
                                
                # Correct the estimated distance by the correction factor
                distance *= self.odom_linear_scale_correction
                
                # How close are we?
                error =  distance - self.test_distance
                
                # Are we close enough?
                if not self.start_test or abs(error) <  self.tolerance:
                    self.start_test = False
                    self.active = 0
                    rospy.loginfo("start_test: False")
                    # Stop the robot
                    self.cmdVelPublisher.publish(Twist())
                else:
                    if self.speed == 0.15:
                        # If not, move in the appropriate direction
                        self.velocity.linear.x = copysign(self.speed, -1 * error)
                    else:
                        # If not, move in the appropriate direction
                        self.velocity.linear.x = copysign(self.speed,  error)
            else:
                self.position = self.get_position()
                x_start = self.position.x
                y_start = self.position.y
                

    

            if(self.active == 1):
                    # self.cmdVelPublisher.publish(self.joyps3)
                self.cmdVelPublisher.publish(self.velocity)
            # print("X : %f  Y : %f  Z: %f ", self.velocity.linear.x , self.velocity.linear.y,self.velocity.angular.z)
            self.rate.sleep()

    def get_position(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return Point(*trans)
        
    def shutdown(self):
        # Always stop the robot when shutting down the node
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)	


if __name__ == '__main__':
    rospy.init_node('show_demo')
    joy = JoyTeleop()
    try:
        rospy.spin()
    except	rospy.ROSInterruptException:
        print('exception')
			
	
